/**
 * @file bmi270.c
 * @brief BMI270 6轴IMU驱动实现
 *
 * 参考PX4实现，支持FIFO模式批量读取
 */

#include "bmi270.h"
#include "bmi270_config.h"  /* Configuration file */
#include "bsp_spi.h"
#include "stm32h7xx_hal.h"
#include "FreeRTOS.h"
#include "task.h"
#include <stddef.h>
#include <string.h>
#include <stdio.h>

/* 内部函数声明 */
static int BMI270_WriteReg(uint8_t reg, uint8_t value);
static int BMI270_ReadReg(uint8_t reg, uint8_t *value);
static int BMI270_ReadRegs(uint8_t reg, uint8_t *buf, uint16_t len);

/**
 * @brief 写BMI270寄存器
 */
static int BMI270_WriteReg(uint8_t reg, uint8_t value)
{
    uint8_t tx_buf[2] = {reg & 0x7F, value};  /* 写操作：地址bit7=0 */

    BSP_SPI_BMI270_CS(1);
    int ret = BSP_SPI_Transfer(SPI_PORT_3, tx_buf, NULL, 2);
    BSP_SPI_BMI270_CS(0);

    HAL_Delay(1);  /* BMI270需要延时 */
    return ret;
}

/**
 * @brief 读BMI270单个寄存器
 * 注意：BMI270读取需要3个字节（地址 + dummy + 数据），数据在第3个字节！
 */
static int BMI270_ReadReg(uint8_t reg, uint8_t *value)
{
    uint8_t tx_buf[3] = {reg | 0x80, 0x00, 0x00};  /* 读操作：地址bit7=1 + dummy + dummy */
    uint8_t rx_buf[3] = {0xFF, 0xFF, 0xFF};  /* 初始化为0xFF以便观察 */

    BSP_SPI_BMI270_CS(1);
    int ret = BSP_SPI_Transfer(SPI_PORT_3, tx_buf, rx_buf, 3);  /* 3字节传输！ */
    BSP_SPI_BMI270_CS(0);

    if (ret == 0 && value != NULL) {
        *value = rx_buf[2];  /* 数据在第3个字节！ */
    }

    return ret;
}

/**
 * @brief 读BMI270多个寄存器
 * 注意：BMI270读取需要dummy字节，数据从第3个字节开始！
 */
static int BMI270_ReadRegs(uint8_t reg, uint8_t *buf, uint16_t len)
{
    if (buf == NULL || len == 0) {
        return -1;
    }

    uint8_t tx_buf[256] = {0};
    uint8_t rx_buf[256] = {0};

    if (len + 2 > sizeof(tx_buf)) {  /* 需要地址+dummy+数据 */
        return -1;
    }

    tx_buf[0] = reg | 0x80;  /* 读操作：地址bit7=1 */
    /* tx_buf[1] = dummy byte */

    BSP_SPI_BMI270_CS(1);
    int ret = BSP_SPI_Transfer(SPI_PORT_3, tx_buf, rx_buf, len + 2);  /* +2: 地址+dummy */
    BSP_SPI_BMI270_CS(0);

    if (ret == 0) {
        memcpy(buf, &rx_buf[2], len);  /* 数据从第3个字节开始！ */
    }

    return ret;
}

/**
 * @brief 初始化BMI270
 * @return 0成功，-1失败，-2 Chip ID错误
 */
int BMI270_Init(void)
{
    uint8_t chip_id = 0;

    /* 延时等待芯片启动 */
    HAL_Delay(10);

    /* BMI270特殊要求：
     * 根据数据手册第123页，BMI270上电后默认为I2C模式，
     * 需要在CSB引脚上产生一个上升沿来切换到SPI模式。
     * 方法：执行一次dummy SPI读取操作（获得的值无效）
     * 参考：PX4 BMI270.cpp probe()函数
     */
    BMI270_ReadReg(BMI270_CHIP_ID, &chip_id);  /* Dummy read to switch to SPI mode */

    /* 等待200us让设备启动SPI通信 */
    for (volatile int i = 0; i < 2000; i++);  /* 约200us @ 480MHz */

    /* 读取Chip ID验证 */
    if (BMI270_ReadReg(BMI270_CHIP_ID, &chip_id) != 0) {
        return -1;  /* SPI读取失败 */
    }

    if (chip_id != BMI270_CHIP_ID_VALUE) {
        return -2;  /* Chip ID不匹配 */
    }

    /* 软复位（PX4时序） */
    BMI270_WriteReg(BMI270_CMD, BMI270_SOFT_RESET_CMD);
    HAL_Delay(1);  /* PX4等待1ms */

    /* 重要：软复位后设备会回到I2C模式，需要执行dummy read切换到SPI模式
     * PX4通过读取CHIP_ID来实现dummy read，同时验证复位是否完成
     * 如果CHIP_ID不匹配，说明复位未完成，需要重试
     */
    uint8_t reset_chip_id = 0;
    int reset_retry = 0;
    const int max_reset_retry = 100;  /* 最多重试100次（约1000ms） */

    while (reset_retry < max_reset_retry) {
        /* Dummy read切换到SPI模式 */
        BMI270_ReadReg(BMI270_CHIP_ID, &reset_chip_id);
        for (volatile int i = 0; i < 2000; i++);  /* 等待200us */

        /* 再次读取验证 */
        BMI270_ReadReg(BMI270_CHIP_ID, &reset_chip_id);

        if (reset_chip_id == 0x24) {
            break;  /* 复位完成，CHIP_ID匹配 */
        }

        HAL_Delay(10);  /* 等待10ms后重试 */
        reset_retry++;
    }

    if (reset_chip_id != 0x24) {
        return -2;  /* 复位后CHIP_ID不匹配 */
    }

    /* 禁用高级省电模式，然后等待450us（PX4时序） */
    BMI270_WriteReg(BMI270_PWR_CONF, 0x00);
    for (volatile int i = 0; i < 4500; i++);  /* 等待450us @ 480MHz */

    /* 上传配置文件（microcode）- 这是BMI270正常工作的关键！ */
    /* PX4时序：
     * 1. 写0x00到INIT_CTRL (0x59)
     * 2. 立即写配置文件到INIT_DATA (0x5E) - 无需额外延时！
     * 3. 写0x01到INIT_CTRL (0x59)
     * 4. 等待150ms
     * 5. 检查INTERNAL_STATUS (0x21) bit0 = 1
     */

    /* 1. 写0x00到INIT_CTRL */
    BMI270_WriteReg(BMI270_INIT_CTRL, 0x00);

    /* 2. 立即写配置文件到INIT_DATA（PX4没有在这里延时！） */
    /* 配置文件的第一个字节是寄存器地址(0x5E)，后面是数据 */
    BSP_SPI_BMI270_CS(1);
    int ret = BSP_SPI_Transfer(SPI_PORT_3, (uint8_t*)bmi270_maximum_fifo_config_file, NULL, BMI270_CONFIG_FILE_SIZE);
    BSP_SPI_BMI270_CS(0);

    if (ret != 0) {
        return -3;  /* 配置文件上传失败 */
    }

    /* 3. 写0x01到INIT_CTRL */
    BMI270_WriteReg(BMI270_INIT_CTRL, 0x01);

    /* 4. 等待150ms */
    HAL_Delay(150);

    /* 5. 检查INTERNAL_STATUS */
    uint8_t internal_status = 0;
    BMI270_ReadReg(BMI270_INTERNAL_STATUS, &internal_status);

    if ((internal_status & 0x01) != 0x01) {
        return -4;  /* 初始化失败，INTERNAL_STATUS bit0 != 1 */
    }

    /* 配置加速度计：±16g, 1600Hz, Normal mode */
    BMI270_WriteReg(BMI270_ACC_CONF, BMI270_ACC_ODR_1600HZ | BMI270_ACC_BWP_NORMAL);
    BMI270_WriteReg(BMI270_ACC_RANGE, BMI270_ACC_RANGE_16G);

    /* 配置陀螺仪：±2000dps, 1600Hz, Normal mode */
    BMI270_WriteReg(BMI270_GYR_CONF, BMI270_GYR_ODR_1600HZ | BMI270_GYR_BWP_NORMAL);
    BMI270_WriteReg(BMI270_GYR_RANGE, BMI270_GYR_RANGE_2000DPS);

    /* 使能加速度计和陀螺仪 */
    BMI270_WriteReg(BMI270_PWR_CTRL, BMI270_PWR_CTRL_ACC_EN | BMI270_PWR_CTRL_GYR_EN | BMI270_PWR_CTRL_TEMP_EN);
    HAL_Delay(50);  /* 等待传感器启动 */



    return 0;
}

/**
 * @brief 读取BMI270 Chip ID（用于调试）
 * @return Chip ID值，失败返回0xFF
 */
uint8_t BMI270_ReadChipID(void)
{
    uint8_t chip_id = 0xFF;

    /* 尝试多次读取 */
    for (int i = 0; i < 3; i++) {
        BMI270_ReadReg(BMI270_CHIP_ID, &chip_id);
        if (chip_id != 0x00 && chip_id != 0xFF) {
            break;  /* 读取到有效数据 */
        }
        HAL_Delay(10);
    }

    return chip_id;
}

/**
 * @brief BMI270详细调试函数
 * @param debug_buf 调试信息缓冲区（至少512字节）
 * @param buf_size 缓冲区大小
 * @return 调试信息长度
 */
int BMI270_DebugInfo(char *debug_buf, int buf_size)
{
    if (debug_buf == NULL || buf_size < 512) {
        return -1;
    }

    int len = 0;
    uint8_t tx_buf[10], rx_buf[10];

    len += snprintf(debug_buf + len, buf_size - len, "\r\n=== BMI270 SPI3 Debug Info ===\r\n");

    /* 测试1：读取多个寄存器 */
    len += snprintf(debug_buf + len, buf_size - len, "Test 1: Read Chip ID (0x00) multiple times:\r\n");
    for (int i = 0; i < 5; i++) {
        tx_buf[0] = 0x80;  /* Read Chip ID */
        tx_buf[1] = 0x00;
        rx_buf[0] = 0xFF;
        rx_buf[1] = 0xFF;

        BSP_SPI_BMI270_CS(1);
        HAL_Delay(1);
        BSP_SPI_Transfer(SPI_PORT_3, tx_buf, rx_buf, 2);
        HAL_Delay(1);
        BSP_SPI_BMI270_CS(0);

        len += snprintf(debug_buf + len, buf_size - len,
                       "  [%d] TX: 0x%02X 0x%02X -> RX: 0x%02X 0x%02X\r\n",
                       i, tx_buf[0], tx_buf[1], rx_buf[0], rx_buf[1]);
        HAL_Delay(10);
    }

    /* 测试2：读取其他寄存器 */
    len += snprintf(debug_buf + len, buf_size - len, "\r\nTest 2: Read other registers:\r\n");
    uint8_t test_regs[] = {0x00, 0x7C, 0x7D, 0x7E};  /* CHIP_ID, PWR_CONF, PWR_CTRL, CMD */
    for (int i = 0; i < 4; i++) {
        tx_buf[0] = test_regs[i] | 0x80;
        tx_buf[1] = 0x00;
        rx_buf[0] = 0xFF;
        rx_buf[1] = 0xFF;

        BSP_SPI_BMI270_CS(1);
        HAL_Delay(1);
        BSP_SPI_Transfer(SPI_PORT_3, tx_buf, rx_buf, 2);
        HAL_Delay(1);
        BSP_SPI_BMI270_CS(0);

        len += snprintf(debug_buf + len, buf_size - len,
                       "  Reg 0x%02X: TX: 0x%02X 0x%02X -> RX: 0x%02X 0x%02X\r\n",
                       test_regs[i], tx_buf[0], tx_buf[1], rx_buf[0], rx_buf[1]);
        HAL_Delay(10);
    }

    /* 测试3：尝试写入然后读回 */
    len += snprintf(debug_buf + len, buf_size - len, "\r\nTest 3: Write and read back test:\r\n");

    /* 写入PWR_CONF寄存器 */
    tx_buf[0] = 0x7C;  /* PWR_CONF address */
    tx_buf[1] = 0x00;  /* Disable advanced power save */
    BSP_SPI_BMI270_CS(1);
    HAL_Delay(1);
    BSP_SPI_Transfer(SPI_PORT_3, tx_buf, NULL, 2);
    HAL_Delay(1);
    BSP_SPI_BMI270_CS(0);
    HAL_Delay(10);

    /* 读回PWR_CONF寄存器 */
    tx_buf[0] = 0x7C | 0x80;
    tx_buf[1] = 0x00;
    rx_buf[0] = 0xFF;
    rx_buf[1] = 0xFF;
    BSP_SPI_BMI270_CS(1);
    HAL_Delay(1);
    BSP_SPI_Transfer(SPI_PORT_3, tx_buf, rx_buf, 2);
    HAL_Delay(1);
    BSP_SPI_BMI270_CS(0);

    len += snprintf(debug_buf + len, buf_size - len,
                   "  Write 0x00 to PWR_CONF (0x7C)\r\n");
    len += snprintf(debug_buf + len, buf_size - len,
                   "  Read back: RX: 0x%02X 0x%02X (expected: 0xXX 0x00)\r\n",
                   rx_buf[0], rx_buf[1]);

    len += snprintf(debug_buf + len, buf_size - len, "\r\n=== End of Debug Info ===\r\n\r\n");

    return len;
}

/**
 * @brief 读取BMI270数据
 */
int BMI270_ReadData(BMI270_Data_t *data)
{
    if (data == NULL) {
        return -1;
    }

    uint8_t buf[12];  /* 6轴数据：6*2=12字节 */

    /* 读取加速度计和陀螺仪数据 */
    if (BMI270_ReadRegs(BMI270_ACC_DATA_X_LSB, buf, 12) != 0) {
        return -1;
    }

    /* 解析加速度计数据 */
    int16_t acc_x = (int16_t)((buf[1] << 8) | buf[0]);
    int16_t acc_y = (int16_t)((buf[3] << 8) | buf[2]);
    int16_t acc_z = (int16_t)((buf[5] << 8) | buf[4]);

    /* 解析陀螺仪数据 */
    int16_t gyr_x = (int16_t)((buf[7] << 8) | buf[6]);
    int16_t gyr_y = (int16_t)((buf[9] << 8) | buf[8]);
    int16_t gyr_z = (int16_t)((buf[11] << 8) | buf[10]);

    /* 转换为物理单位 */
    data->accel_x = acc_x * BMI270_ACC_SCALE_16G;
    data->accel_y = acc_y * BMI270_ACC_SCALE_16G;
    data->accel_z = acc_z * BMI270_ACC_SCALE_16G;

    data->gyro_x = gyr_x * BMI270_GYR_SCALE_2000DPS;
    data->gyro_y = gyr_y * BMI270_GYR_SCALE_2000DPS;
    data->gyro_z = gyr_z * BMI270_GYR_SCALE_2000DPS;

    /* 读取温度 */
    data->temperature = BMI270_ReadTemperature();

    return 0;
}

/**
 * @brief 读取BMI270温度
 */
float BMI270_ReadTemperature(void)
{
    uint8_t buf[2];

    if (BMI270_ReadRegs(BMI270_TEMP_LSB, buf, 2) != 0) {
        return 0.0f;
    }

    /* 组合温度数据（11位有符号整数） */
    int16_t temp_raw = (int16_t)((buf[1] << 8) | buf[0]);

    /* 转换为温度值 */
    float temperature = (temp_raw * BMI270_TEMP_SCALE) + BMI270_TEMP_OFFSET;

    return temperature;
}

/**
 * @brief 初始化BMI270 FIFO模式
 * @param watermark FIFO水印（字节数）
 * @return 0成功，-1失败
 */
int BMI270_FIFO_Init(uint16_t watermark)
{
    /* 配置FIFO水印 */
    BMI270_WriteReg(BMI270_FIFO_WTM_0, watermark & 0xFF);
    BMI270_WriteReg(BMI270_FIFO_WTM_1, (watermark >> 8) & 0x07);

    /* 配置FIFO：使能加速度计和陀螺仪
     * FIFO_CONFIG_0: Bit1必须为1（BIT1_ALWAYS），Bit0=1（FIFO模式）
     * FIFO_CONFIG_1: Bit7=1（Gyr_en），Bit6=1（Acc_en），Bit4必须为1（BIT4_ALWAYS）
     */
    BMI270_WriteReg(BMI270_FIFO_CONFIG_0, 0x03);  /* Bit1 | Bit0 = BIT1_ALWAYS | FIFO_mode */
    BMI270_WriteReg(BMI270_FIFO_CONFIG_1, 0xD0);  /* Bit7 | Bit6 | Bit4 = Gyr_en | Acc_en | BIT4_ALWAYS */

    /* 优先级3B：配置INT1引脚为FIFO水印中断 */
    /* 配置INT1为输出，推挽模式，高电平有效 */
    BMI270_WriteReg(BMI270_INT1_IO_CTRL, BMI270_INT1_OUT);  /* INT1输出使能 */

    /* 映射FIFO水印中断到INT1 */
    BMI270_WriteReg(BMI270_INT_MAP_DATA, BMI270_INT1_FWM);  /* 映射FIFO水印到INT1 */

    return 0;
}

/**
 * @brief 同时读取加速度计和陀螺仪FIFO数据
 *
 * 这是推荐的方法，因为BMI270的FIFO包含组合帧（加速度计+陀螺仪）
 * 读取一次FIFO可以同时获取两种数据，避免重复读取导致数据丢失
 *
 * @param fifo_data 输出参数，包含加速度计和陀螺仪数据
 * @return 0成功，-1失败
 */
int BMI270_FIFO_Read(BMI270_FIFO_Data_t *fifo_data)
{
    if (fifo_data == NULL) {
        return -1;
    }

    /* 读取FIFO长度 */
    uint16_t fifo_length = BMI270_FIFO_GetLength();

    if (fifo_length == 0) {
        fifo_data->accel.accel_count = 0;
        fifo_data->gyro.gyro_count = 0;
        return 0;  /* FIFO为空 */
    }

    /* 限制读取长度到最大样本数对应的字节数
     * 组合帧：13字节（1字节帧头 + 6字节陀螺仪 + 6字节加速度计）
     * 最大字节数 = BMI270_FIFO_MAX_SAMPLES * 13 = 32 * 13 = 416字节
     * 但为了安全，我们限制到224字节（与PX4一致）
     */
    const uint16_t max_fifo_bytes = BMI270_FIFO_MAX_SAMPLES * 7;
    if (fifo_length > max_fifo_bytes) {
        fifo_length = max_fifo_bytes;
    }

    /* 在栈上分配缓冲区 - 最大224字节，安全 */
    uint8_t fifo_buf[BMI270_FIFO_MAX_SAMPLES * 7];
    if (BMI270_ReadRegs(BMI270_FIFO_DATA, fifo_buf, fifo_length) != 0) {
        return -1;
    }

    /* 解析FIFO数据 - 同时解析加速度计和陀螺仪 */
    fifo_data->accel.accel_count = 0;
    fifo_data->gyro.gyro_count = 0;
    uint16_t index = 0;

    while (index < fifo_length) {
        uint8_t header = fifo_buf[index] & BMI270_FIFO_HEADER_MASK;

        if (header == BMI270_FIFO_HEADER_ACC_GYR) {
            /* 组合帧：1字节帧头 + 6字节陀螺仪数据 + 6字节加速度计数据 = 13字节 */
            if (index + 13 > fifo_length) break;

            /* 解析陀螺仪数据（字节1-6） */
            if (fifo_data->gyro.gyro_count < BMI270_FIFO_MAX_SAMPLES) {
                int16_t gx = (int16_t)((fifo_buf[index + 2] << 8) | fifo_buf[index + 1]);
                int16_t gy = (int16_t)((fifo_buf[index + 4] << 8) | fifo_buf[index + 3]);
                int16_t gz = (int16_t)((fifo_buf[index + 6] << 8) | fifo_buf[index + 5]);

                fifo_data->gyro.gyro_x[fifo_data->gyro.gyro_count] = gx;
                fifo_data->gyro.gyro_y[fifo_data->gyro.gyro_count] = gy;
                fifo_data->gyro.gyro_z[fifo_data->gyro.gyro_count] = gz;
                fifo_data->gyro.gyro_count++;
            }

            /* 解析加速度计数据（字节7-12） */
            if (fifo_data->accel.accel_count < BMI270_FIFO_MAX_SAMPLES) {
                int16_t ax = (int16_t)((fifo_buf[index + 8] << 8) | fifo_buf[index + 7]);
                int16_t ay = (int16_t)((fifo_buf[index + 10] << 8) | fifo_buf[index + 9]);
                int16_t az = (int16_t)((fifo_buf[index + 12] << 8) | fifo_buf[index + 11]);

                fifo_data->accel.accel_x[fifo_data->accel.accel_count] = ax;
                fifo_data->accel.accel_y[fifo_data->accel.accel_count] = ay;
                fifo_data->accel.accel_z[fifo_data->accel.accel_count] = az;
                fifo_data->accel.accel_count++;
            }

            index += 13;
        } else if (header == BMI270_FIFO_HEADER_ACC) {
            /* 加速度计帧：1字节帧头 + 6字节数据 */
            if (index + 7 > fifo_length) break;

            if (fifo_data->accel.accel_count < BMI270_FIFO_MAX_SAMPLES) {
                int16_t ax = (int16_t)((fifo_buf[index + 2] << 8) | fifo_buf[index + 1]);
                int16_t ay = (int16_t)((fifo_buf[index + 4] << 8) | fifo_buf[index + 3]);
                int16_t az = (int16_t)((fifo_buf[index + 6] << 8) | fifo_buf[index + 5]);

                fifo_data->accel.accel_x[fifo_data->accel.accel_count] = ax;
                fifo_data->accel.accel_y[fifo_data->accel.accel_count] = ay;
                fifo_data->accel.accel_z[fifo_data->accel.accel_count] = az;
                fifo_data->accel.accel_count++;
            }

            index += 7;
        } else if (header == BMI270_FIFO_HEADER_GYR) {
            /* 陀螺仪帧：1字节帧头 + 6字节数据 */
            if (index + 7 > fifo_length) break;

            if (fifo_data->gyro.gyro_count < BMI270_FIFO_MAX_SAMPLES) {
                int16_t gx = (int16_t)((fifo_buf[index + 2] << 8) | fifo_buf[index + 1]);
                int16_t gy = (int16_t)((fifo_buf[index + 4] << 8) | fifo_buf[index + 3]);
                int16_t gz = (int16_t)((fifo_buf[index + 6] << 8) | fifo_buf[index + 5]);

                fifo_data->gyro.gyro_x[fifo_data->gyro.gyro_count] = gx;
                fifo_data->gyro.gyro_y[fifo_data->gyro.gyro_count] = gy;
                fifo_data->gyro.gyro_z[fifo_data->gyro.gyro_count] = gz;
                fifo_data->gyro.gyro_count++;
            }

            index += 7;
        } else if (header == BMI270_FIFO_HEADER_SKIP) {
            index += 1;
        } else {
            /* 未知帧头，停止解析 */
            break;
        }
    }

    return 0;
}

/**
 * @brief 读取加速度计FIFO数据
 *
 * 注意：此函数已弃用，请使用BMI270_FIFO_Read()代替
 * 因为BMI270的FIFO包含组合帧，分别读取会导致数据丢失
 *
 * PX4方案：只读取需要的字节数，不读取整个FIFO
 * 最大缓冲区大小 = BMI270_FIFO_MAX_SAMPLES * 7字节（1字节帧头 + 6字节数据）= 32 * 7 = 224字节
 */
int BMI270_FIFO_ReadAccel(BMI270_FIFO_AccelData_t *fifo_data)
{
    if (fifo_data == NULL) {
        return -1;
    }

    /* 读取FIFO长度 */
    uint8_t len_buf[2];
    if (BMI270_ReadRegs(BMI270_FIFO_LENGTH_0, len_buf, 2) != 0) {
        return -1;
    }

    /* FIFO_LENGTH是14位：FIFO_LENGTH_1[5:0]（高6位）+ FIFO_LENGTH_0[7:0]（低8位） */
    uint16_t fifo_length = ((uint16_t)(len_buf[1] & 0x3F) << 8) | len_buf[0];

    if (fifo_length == 0) {
        return 0;  /* FIFO为空 */
    }

    /* 限制读取长度到最大样本数对应的字节数
     * 最大字节数 = BMI270_FIFO_MAX_SAMPLES * 7（帧头+数据）= 224字节
     * 这样可以安全地在栈上分配缓冲区
     */
    const uint16_t max_fifo_bytes = BMI270_FIFO_MAX_SAMPLES * 7;
    if (fifo_length > max_fifo_bytes) {
        fifo_length = max_fifo_bytes;
    }

    /* 在栈上分配缓冲区 - 最大224字节，安全 */
    uint8_t fifo_buf[BMI270_FIFO_MAX_SAMPLES * 7];
    if (BMI270_ReadRegs(BMI270_FIFO_DATA, fifo_buf, fifo_length) != 0) {
        return -1;
    }

    /* 解析FIFO数据 */
    fifo_data->accel_count = 0;
    uint16_t index = 0;

    while (index < fifo_length && fifo_data->accel_count < BMI270_FIFO_MAX_SAMPLES) {
        uint8_t header = fifo_buf[index] & BMI270_FIFO_HEADER_MASK;

        if (header == BMI270_FIFO_HEADER_ACC_GYR) {
            /* 组合帧：1字节帧头 + 6字节陀螺仪数据 + 6字节加速度计数据 = 13字节 */
            if (index + 13 > fifo_length) break;

            /* 跳过帧头(1字节)和陀螺仪数据(6字节)，读取加速度计数据 */
            int16_t x = (int16_t)((fifo_buf[index + 8] << 8) | fifo_buf[index + 7]);
            int16_t y = (int16_t)((fifo_buf[index + 10] << 8) | fifo_buf[index + 9]);
            int16_t z = (int16_t)((fifo_buf[index + 12] << 8) | fifo_buf[index + 11]);

            fifo_data->accel_x[fifo_data->accel_count] = x;
            fifo_data->accel_y[fifo_data->accel_count] = y;
            fifo_data->accel_z[fifo_data->accel_count] = z;
            fifo_data->accel_count++;

            index += 13;
        } else if (header == BMI270_FIFO_HEADER_ACC) {
            /* 加速度计帧：1字节帧头 + 6字节数据 */
            if (index + 7 > fifo_length) break;

            int16_t x = (int16_t)((fifo_buf[index + 2] << 8) | fifo_buf[index + 1]);
            int16_t y = (int16_t)((fifo_buf[index + 4] << 8) | fifo_buf[index + 3]);
            int16_t z = (int16_t)((fifo_buf[index + 6] << 8) | fifo_buf[index + 5]);

            fifo_data->accel_x[fifo_data->accel_count] = x;
            fifo_data->accel_y[fifo_data->accel_count] = y;
            fifo_data->accel_z[fifo_data->accel_count] = z;
            fifo_data->accel_count++;

            index += 7;
        } else if (header == BMI270_FIFO_HEADER_GYR) {
            /* 陀螺仪帧：跳过 */
            index += 7;
        } else if (header == BMI270_FIFO_HEADER_SKIP) {
            index += 1;
        } else {
            /* 未知帧头，停止解析 */
            break;
        }
    }

    return fifo_data->accel_count;
}

/**
 * @brief 读取陀螺仪FIFO数据
 *
 * PX4方案：只读取需要的字节数，不读取整个FIFO
 * 最大缓冲区大小 = BMI270_FIFO_MAX_SAMPLES * 7字节（1字节帧头 + 6字节数据）= 32 * 7 = 224字节
 */
int BMI270_FIFO_ReadGyro(BMI270_FIFO_GyroData_t *fifo_data)
{
    if (fifo_data == NULL) {
        return -1;
    }

    /* 读取FIFO长度 */
    uint8_t len_buf[2];
    if (BMI270_ReadRegs(BMI270_FIFO_LENGTH_0, len_buf, 2) != 0) {
        return -1;
    }

    /* FIFO_LENGTH是14位：FIFO_LENGTH_1[5:0]（高6位）+ FIFO_LENGTH_0[7:0]（低8位） */
    uint16_t fifo_length = ((uint16_t)(len_buf[1] & 0x3F) << 8) | len_buf[0];

    if (fifo_length == 0) {
        return 0;  /* FIFO为空 */
    }

    /* 限制读取长度到最大样本数对应的字节数
     * 最大字节数 = BMI270_FIFO_MAX_SAMPLES * 7（帧头+数据）= 224字节
     * 这样可以安全地在栈上分配缓冲区
     */
    const uint16_t max_fifo_bytes = BMI270_FIFO_MAX_SAMPLES * 7;
    if (fifo_length > max_fifo_bytes) {
        fifo_length = max_fifo_bytes;
    }

    /* 在栈上分配缓冲区 - 最大224字节，安全 */
    uint8_t fifo_buf[BMI270_FIFO_MAX_SAMPLES * 7];
    if (BMI270_ReadRegs(BMI270_FIFO_DATA, fifo_buf, fifo_length) != 0) {
        return -1;
    }

    /* 解析FIFO数据 */
    fifo_data->gyro_count = 0;
    uint16_t index = 0;

    while (index < fifo_length && fifo_data->gyro_count < BMI270_FIFO_MAX_SAMPLES) {
        uint8_t header = fifo_buf[index] & BMI270_FIFO_HEADER_MASK;

        if (header == BMI270_FIFO_HEADER_ACC_GYR) {
            /* 组合帧：1字节帧头 + 6字节陀螺仪数据 + 6字节加速度计数据 = 13字节 */
            if (index + 13 > fifo_length) break;

            /* 跳过帧头(1字节)，读取陀螺仪数据 */
            int16_t x = (int16_t)((fifo_buf[index + 2] << 8) | fifo_buf[index + 1]);
            int16_t y = (int16_t)((fifo_buf[index + 4] << 8) | fifo_buf[index + 3]);
            int16_t z = (int16_t)((fifo_buf[index + 6] << 8) | fifo_buf[index + 5]);

            fifo_data->gyro_x[fifo_data->gyro_count] = x;
            fifo_data->gyro_y[fifo_data->gyro_count] = y;
            fifo_data->gyro_z[fifo_data->gyro_count] = z;
            fifo_data->gyro_count++;

            index += 13;
        } else if (header == BMI270_FIFO_HEADER_GYR) {
            /* 陀螺仪帧：1字节帧头 + 6字节数据 */
            if (index + 7 > fifo_length) break;

            int16_t x = (int16_t)((fifo_buf[index + 2] << 8) | fifo_buf[index + 1]);
            int16_t y = (int16_t)((fifo_buf[index + 4] << 8) | fifo_buf[index + 3]);
            int16_t z = (int16_t)((fifo_buf[index + 6] << 8) | fifo_buf[index + 5]);

            fifo_data->gyro_x[fifo_data->gyro_count] = x;
            fifo_data->gyro_y[fifo_data->gyro_count] = y;
            fifo_data->gyro_z[fifo_data->gyro_count] = z;
            fifo_data->gyro_count++;

            index += 7;
        } else if (header == BMI270_FIFO_HEADER_ACC) {
            /* 加速度计帧：跳过 */
            index += 7;
        } else if (header == BMI270_FIFO_HEADER_SKIP) {
            index += 1;
        } else {
            /* 未知帧头，停止解析 */
            break;
        }
    }

    return fifo_data->gyro_count;
}

/**
 * @brief 复位FIFO
 */
int BMI270_FIFO_Reset(void)
{
    /* 写CMD寄存器复位FIFO */
    return BMI270_WriteReg(BMI270_CMD, 0xB0);
}

/**
 * @brief 获取FIFO长度
 *
 * PX4方案：使用4字节传输读取FIFO_LENGTH_0和FIFO_LENGTH_1
 * 传输格式：[cmd | dummy | FIFO_LENGTH_0 | FIFO_LENGTH_1]
 */
uint16_t BMI270_FIFO_GetLength(void)
{
    /* 4字节缓冲区：cmd + dummy + FIFO_LENGTH_0 + FIFO_LENGTH_1 */
    uint8_t tx_buf[4] = {BMI270_FIFO_LENGTH_0 | 0x80, 0, 0, 0};
    uint8_t rx_buf[4] = {0};

    BSP_SPI_BMI270_CS(1);
    int ret = BSP_SPI_Transfer(SPI_PORT_3, tx_buf, rx_buf, 4);
    BSP_SPI_BMI270_CS(0);

    if (ret != 0) {
        return 0;
    }

    /* FIFO_LENGTH是14位：FIFO_LENGTH_1[5:0]（高6位）+ FIFO_LENGTH_0[7:0]（低8位） */
    /* rx_buf[2] = FIFO_LENGTH_0, rx_buf[3] = FIFO_LENGTH_1 */
    return ((uint16_t)(rx_buf[3] & 0x3F) << 8) | rx_buf[2];
}

/* ========== 优先级2：重要改进功能 ========== */

/* 全局错误统计 */
static BMI270_ErrorStats_t g_bmi270_error_stats = {0};

/* 寄存器配置表 - 用于定期验证 */
static const BMI270_RegisterConfig_t g_bmi270_register_config[] = {
    /* Register                  | Set bits                                    | Clear bits */
    {BMI270_PWR_CONF,             0x00,                                         0x00},  /* Advanced power save disabled */
    {BMI270_PWR_CTRL,             BMI270_PWR_CTRL_ACC_EN | BMI270_PWR_CTRL_GYR_EN, 0x00},  /* Accel and Gyro enabled */
    {BMI270_ACC_CONF,             BMI270_ACC_ODR_1600HZ | BMI270_ACC_BWP_NORMAL, 0x00},  /* 1600Hz, Normal mode */
    {BMI270_ACC_RANGE,            BMI270_ACC_RANGE_16G,                         0x00},  /* ±16g */
    {BMI270_GYR_CONF,             BMI270_GYR_ODR_1600HZ | BMI270_GYR_BWP_NORMAL, 0x00},  /* 1600Hz, Normal mode */
    {BMI270_GYR_RANGE,            BMI270_GYR_RANGE_2000DPS,                     0x00},  /* ±2000dps */
    {BMI270_FIFO_CONFIG_1,        0x50,                                         0x00},  /* FIFO mode, ACC+GYR enabled */
};

#define BMI270_REGISTER_CONFIG_COUNT (sizeof(g_bmi270_register_config) / sizeof(g_bmi270_register_config[0]))

/**
 * @brief 寄存器配置验证
 *
 * 检查寄存器是否按预期配置，防止意外修改
 *
 * @param reg_cfg 寄存器配置
 * @return 0成功，-1失败
 */
int BMI270_RegisterCheck(const BMI270_RegisterConfig_t *reg_cfg)
{
    if (reg_cfg == NULL) {
        return -1;
    }

    uint8_t reg_value = 0;
    if (BMI270_ReadReg(reg_cfg->reg, &reg_value) != 0) {
        g_bmi270_error_stats.bad_transfer_count++;
        return -1;
    }

    /* 检查应该设置的位 */
    if (reg_cfg->set_bits && ((reg_value & reg_cfg->set_bits) != reg_cfg->set_bits)) {
        g_bmi270_error_stats.bad_register_count++;
        return -1;
    }

    /* 检查应该清除的位 */
    if (reg_cfg->clear_bits && ((reg_value & reg_cfg->clear_bits) != 0)) {
        g_bmi270_error_stats.bad_register_count++;
        return -1;
    }

    return 0;
}

/**
 * @brief 错误寄存器检测
 *
 * 检测BMI270内部错误状态
 *
 * @return 0无错误，>0有错误，-1读取失败
 */
int BMI270_CheckErrorRegister(void)
{
    uint8_t err_reg = 0;
    if (BMI270_ReadReg(BMI270_ERR_REG, &err_reg) != 0) {
        g_bmi270_error_stats.bad_transfer_count++;
        return -1;
    }

    if (err_reg == 0) {
        return 0;  /* 无错误 */
    }

    /* 解析错误位 */
    int error_count = 0;

    /* Bit 0: fatal_err - 致命错误 */
    if (err_reg & 0x01) {
        error_count++;
        /* 读取INTERNAL_STATUS获取详细信息 */
        uint8_t status = 0;
        BMI270_ReadReg(BMI270_INTERNAL_STATUS, &status);
        /* 可以根据status进一步分析错误类型 */
    }

    /* Bit 1: cmd_err - 命令错误 */
    if (err_reg & 0x02) {
        error_count++;
    }

    /* Bit 2-4: err_code - 错误代码 */
    uint8_t err_code = (err_reg >> 2) & 0x07;
    if (err_code != 0) {
        error_count++;
    }

    /* Bit 5: fifo_err - FIFO错误 */
    if (err_reg & 0x20) {
        error_count++;
        g_bmi270_error_stats.fifo_overflow_count++;
    }

    /* Bit 6: aux_err - 辅助传感器错误 */
    if (err_reg & 0x40) {
        error_count++;
    }

    return error_count;
}

/**
 * @brief 温度读取
 *
 * 读取BMI270温度传感器数据
 *
 * @param temperature 输出温度值（°C）
 * @return 0成功，-1失败
 */
int BMI270_UpdateTemperature(float *temperature)
{
    if (temperature == NULL) {
        return -1;
    }

    /* 读取温度寄存器（2字节） */
    uint8_t temp_buf[4] = {0};
    temp_buf[0] = BMI270_TEMP_MSB | 0x80;

    BSP_SPI_BMI270_CS(1);
    int ret = BSP_SPI_Transfer(SPI_PORT_3, temp_buf, temp_buf, 4);
    BSP_SPI_BMI270_CS(0);

    if (ret != 0) {
        g_bmi270_error_stats.bad_transfer_count++;
        return -1;
    }

    /* 组合温度值：temp_buf[2] = LSB, temp_buf[3] = MSB */
    uint16_t temp_raw = temp_buf[2] | (temp_buf[3] << 8);

    /* 检查无效值 */
    if (temp_raw == 0x8000) {
        *temperature = 0.0f;  /* 无效温度 */
        return -1;
    }

    /* 温度转换公式：Temperature = 23 + (int16_t)temp_raw / 512 */
    /* 或者：Temperature = 23 + (int16_t)temp_raw * 0.001953125 */
    *temperature = 23.0f + ((int16_t)temp_raw) / 512.0f;

    return 0;
}

/**
 * @brief 获取错误统计
 *
 * @param stats 输出错误统计数据
 */
void BMI270_GetErrorStats(BMI270_ErrorStats_t *stats)
{
    if (stats != NULL) {
        *stats = g_bmi270_error_stats;
    }
}

/**
 * @brief 重置错误统计
 */
void BMI270_ResetErrorStats(void)
{
    memset(&g_bmi270_error_stats, 0, sizeof(g_bmi270_error_stats));
}

/* ========== 优先级3：高级功能 ========== */

/* 全局状态机变量 */
static BMI270_State_t g_bmi270_state = BMI270_STATE_RESET;
static uint32_t g_bmi270_failure_count = 0;
static uint32_t g_bmi270_reset_timestamp = 0;  /* 复位时间戳（用于超时检测） */

/**
 * @brief 软复位BMI270传感器
 *
 * 模仿PX4设计：写入0xB6到CMD寄存器进行软复位
 *
 * @return 0成功，-1失败
 */
int BMI270_Reset(void)
{
    /* 写入软复位命令 */
    if (BMI270_WriteReg(BMI270_CMD, 0xB6) != 0) {
        g_bmi270_error_stats.bad_transfer_count++;
        return -1;
    }

    /* 更新状态和时间戳 */
    g_bmi270_state = BMI270_STATE_WAIT_FOR_RESET;
    g_bmi270_reset_timestamp = HAL_GetTick();
    g_bmi270_failure_count = 0;

    return 0;
}

/**
 * @brief 获取当前状态
 */
BMI270_State_t BMI270_GetState(void)
{
    return g_bmi270_state;
}

/**
 * @brief 状态机处理函数
 *
 * 模仿PX4的状态机设计，每次调用推进状态
 *
 * @return 0成功，-1失败，1需要等待
 */
int BMI270_StateMachine(void)
{
    uint32_t now = HAL_GetTick();

    switch (g_bmi270_state) {
    case BMI270_STATE_RESET:
        /* 执行软复位 */
        if (BMI270_Reset() == 0) {
            return 1;  /* 需要等待复位完成 */
        } else {
            return -1;  /* 复位失败 */
        }

    case BMI270_STATE_WAIT_FOR_RESET:
        /* 等待复位完成（至少1ms） */
        if ((now - g_bmi270_reset_timestamp) >= 1) {
            /* 检查CHIP_ID确认复位完成 */
            uint8_t chip_id = BMI270_ReadChipID();
            if (chip_id == 0x24) {
                /* 复位成功，进入微代码加载状态 */
                g_bmi270_state = BMI270_STATE_MICROCODE_LOAD;
                return 0;
            } else if ((now - g_bmi270_reset_timestamp) > 1000) {
                /* 超时（1秒），复位失败 */
                g_bmi270_state = BMI270_STATE_RESET;
                g_bmi270_failure_count++;
                return -1;
            } else {
                /* 继续等待 */
                return 1;
            }
        }
        return 1;  /* 继续等待 */

    case BMI270_STATE_MICROCODE_LOAD:
        /* BMI270需要加载配置文件（微代码） */
        /* 注意：这个步骤在我们的初始化中已经完成 */
        /* 这里直接进入配置状态 */
        g_bmi270_state = BMI270_STATE_CONFIGURE;
        return 0;

    case BMI270_STATE_CONFIGURE:
        /* 配置传感器（在我们的初始化中已经完成） */
        /* 这里直接进入FIFO读取状态 */
        g_bmi270_state = BMI270_STATE_FIFO_READ;
        g_bmi270_failure_count = 0;  /* 配置成功，重置失败计数 */
        return 0;

    case BMI270_STATE_FIFO_READ:
        /* 正常运行状态，不需要状态转换 */
        return 0;

    default:
        /* 未知状态，复位 */
        g_bmi270_state = BMI270_STATE_RESET;
        return -1;
    }
}

/**
 * @brief 检查FIFO溢出
 *
 * 读取FIFO长度，如果超过FIFO大小则认为溢出
 *
 * @return 0无溢出，1溢出，-1读取失败
 */
int BMI270_FIFO_CheckOverflow(void)
{
    uint16_t fifo_length = BMI270_FIFO_GetLength();

    if (fifo_length == 0xFFFF) {
        /* 读取失败 */
        g_bmi270_error_stats.bad_transfer_count++;
        return -1;
    }

    /* BMI270 FIFO大小为1024字节 */
    if (fifo_length >= 1024) {
        g_bmi270_error_stats.fifo_overflow_count++;
        return 1;  /* 溢出 */
    }

    return 0;  /* 无溢出 */
}

/**
 * @brief 强制复位FIFO
 *
 * 模仿PX4设计：通过写入FIFO_CONFIG_0和FIFO_CONFIG_1来复位FIFO
 *
 * @return 0成功，-1失败
 */
int BMI270_FIFO_ForceReset(void)
{
    g_bmi270_error_stats.fifo_reset_count++;

    /* 禁用FIFO */
    if (BMI270_WriteReg(BMI270_FIFO_CONFIG_1, 0x00) != 0) {
        g_bmi270_error_stats.bad_transfer_count++;
        return -1;
    }

    HAL_Delay(1);

    /* 重新启用FIFO（加速度计+陀螺仪） */
    if (BMI270_WriteReg(BMI270_FIFO_CONFIG_1, 0x50) != 0) {
        g_bmi270_error_stats.bad_transfer_count++;
        return -1;
    }

    return 0;
}

/**
 * @brief 增加失败计数
 */
void BMI270_IncrementFailureCount(void)
{
    g_bmi270_failure_count++;
}

/**
 * @brief 重置失败计数
 */
void BMI270_ResetFailureCount(void)
{
    g_bmi270_failure_count = 0;
}

/**
 * @brief 获取失败计数
 */
uint32_t BMI270_GetFailureCount(void)
{
    return g_bmi270_failure_count;
}
